//
// Created by mgts on 2016/4/11.
//

#include "ColorObtain.h"
#include<stdio.h>
#include<stdlib.h>
#include <math.h>
int buffNumber = 3;
unsigned char * buff3 = 0;
int buff3ID = 0;

unsigned char * rgbBuff = 0;

int sbl = 10;
unsigned char * buff5X3 = 0;
int buff5X3id = 0;

//分层原素 白板
unsigned char * whitePlaneP = 0;
unsigned char * planePRGB = 0;
int WIDTH = 0;
int HEIGHT = 0;

unsigned char * colorIDs = 0;
int * wScanLine = 0;
int * wScanLineID = 0;
int * hScanLine = 0;
int * hScanLineID = 0;

MyColor colors[20];
void ColorObtain::Init(int width, int height)
{
    WIDTH = width;
    HEIGHT = height;

    memset(colors, 0, sizeof(MyColor) * 20);


    if (!buff3)
    {
        buff3 = new unsigned char[width * height * buffNumber];//最后乘以5是有5页
        memset(buff3, 0, width * height * buffNumber);
    }

    if (!rgbBuff)
    {
        rgbBuff = new unsigned char[width * height * 3];//最后乘以5是有5页
        memset(rgbBuff, 0, width * height * 3);
    }

    if (!buff5X3)
    {
        buff5X3 = new unsigned char[width * height / sbl * 3];//最后乘以5是有5页
        memset(buff5X3, 0, width * height / sbl * 3);
    }

    if(!whitePlaneP)
    {
        whitePlaneP = new unsigned char[width * height / sbl];//最后乘以5是有5页
        memset(whitePlaneP, 0, width * height / sbl);

        planePRGB = new unsigned char[width * height / sbl * 3];
        memset(planePRGB, 0, width * height / sbl * 3);
    }

    if(!colorIDs)
    {
        colorIDs = new unsigned char[width * height / sbl];
        memset(colorIDs, 0, width * height / sbl);

        wScanLine = new int[width / sbl];
        memset(wScanLine, 0, width / sbl);

        wScanLineID = new int[width / sbl];
        memset(wScanLineID, 0, width / sbl);

        hScanLine = new int[height / sbl];
        memset(hScanLine, 0, height / sbl);

        hScanLineID = new int[height / sbl];
        memset(hScanLineID, 0, height / sbl);
    }
}

void SetWhitePlaneP(unsigned char * color, int w, int h, unsigned char V)
{
    int cX = w / 2;
    int cY = h / 2;

    for(int i = cY-1; i > 0; i--)
    {
        int hid = i * w;
        int hid2 = (i + 1) * w;
        unsigned char C = 0;
        unsigned char C1 = 0;
        unsigned char C2 = 0;
        for(int j = cX; j < w - 1; j++)
        {
            int wid = j;
            C = buff5X3[hid + wid + buff5X3id];
            if(C > 150)
                whitePlaneP[hid + wid] = V;
            else
                whitePlaneP[hid + wid] = 0;
        }
        for(int j = cX - 1; j > 0; j--)
        {
            int wid = j;
            C = buff5X3[hid + wid + buff5X3id];
            if(C > 150)
                whitePlaneP[hid + wid] = V;
            else
                whitePlaneP[hid + wid] = 0;
        }

    }
    for(int i = cY; i < h; i++)
    {
        int hid = i * w;
        int hid2 = (i + 1) * w;
        unsigned char C = 0;
        unsigned char C1 = 0;
        unsigned char C2 = 0;
        for(int j = cX; j < w - 1; j++)
        {
            int wid = j;
            C = buff5X3[hid + wid + buff5X3id];
            if(C > 150)
                whitePlaneP[hid + wid] = V;
            else
                whitePlaneP[hid + wid] = 0;
        }
        for(int j = cX - 1; j > 0; j--)
        {
            int wid = j;
            C = buff5X3[hid + wid + buff5X3id];

            if(C > 150)
                whitePlaneP[hid + wid] = V;
            else
                whitePlaneP[hid + wid] = 0;
        }
    }
}

void SetWhitePlanePAddRGB(unsigned char * color, int w, int h, unsigned char T, unsigned char V)
{
    for(int i = 0; i < h; i++)
    {
        int hid = i * w;
        for(int j = 0; j < w; j++)
        {
            int wid = j;
            if(whitePlaneP[hid + wid] == T)
            {
                unsigned char R = planePRGB[hid * 3 + wid * 3 + 0];
                unsigned char G = planePRGB[hid * 3 + wid * 3 + 1];
                unsigned char B = planePRGB[hid * 3 + wid * 3 + 2];

                float minRGB = Min(R, Min(G, B));
                float maxRGB = Max(R, Max(G, B));

                if(maxRGB - minRGB < 30)
                    whitePlaneP[hid + wid] = V;

            }
        }
    }
}

void IS_Outline(unsigned char * color, int w, int h, unsigned char T, unsigned char V)
{
    for(int i = 1; i < h - 1; i++)
    {
        int hid = i * w;
        for (int j = 1; j < w - 1; j++)
        {
            int wid = j;

            unsigned char pix = whitePlaneP[hid + wid];
            unsigned char leftPix = whitePlaneP[hid + wid - 1];
            unsigned char rightPix = whitePlaneP[hid + wid + 1];
            unsigned char topPix = whitePlaneP[(i - 1) * w + wid];
            unsigned char bottonPix = whitePlaneP[(i + 1) * w + wid];

            unsigned char leftTopPix = whitePlaneP[(i - 1) * w + wid - 1];
            unsigned char rightTopPix = whitePlaneP[(i - 1) * w + wid + 1];
            unsigned char leftBottonPix = whitePlaneP[(i + 1) * w + wid - 1];
            unsigned char rightBottonPix = whitePlaneP[(i + 1) * w + wid + 1];

            if(pix == T)
            {
                if(pix > leftPix || pix > rightPix || pix > topPix || pix > bottonPix)
                    whitePlaneP[hid + wid] = V;

                if(pix > leftTopPix || pix > rightTopPix || pix > leftBottonPix || pix > rightBottonPix)
                    whitePlaneP[hid + wid] = V;
            }
        }
    }
}

void IS_SideW(unsigned char * color, int w, int h, unsigned char T, unsigned char V)
{
    for(int i = 1; i < h - 1; i++)
    {
        int hid = i * w;
        for (int j = 1; j < w - 1; j++)
        {
            int wid = j;

            unsigned char pix = whitePlaneP[hid + wid];
            if(pix == T)
            {
                whitePlaneP[hid + wid] = V;
                break;
            }
        }
        for (int j = w - 1; j > 0; j--)
        {
            int wid = j;
            unsigned char pix = whitePlaneP[hid + wid];
            if(pix == T)
            {
                whitePlaneP[hid + wid] = V;
                break;
            }
        }
    }
}

void IS_SideH(unsigned char * color, int w, int h, unsigned char T, unsigned char V)
{
    for(int j = 0; j < w; j++)
    {
        int wid = j;
        for(int i = 0; i < h; i++)
        {
            int hid = i * w;
            unsigned char pix = whitePlaneP[hid + wid];
            if(pix == T)
            {
                whitePlaneP[hid + wid] = V;
                break;
            }
        }
        for(int i = h - 1; i > 0; i--)
        {
            int hid = i * w;
            unsigned char pix = whitePlaneP[hid + wid];
            if(pix == T)
            {
                whitePlaneP[hid + wid] = V;
                break;
            }
        }
    }
}

void IS_Block(unsigned char * color, int w, int h, unsigned char T1, unsigned char T2, unsigned char V)
{
    for(int i = 1; i < h - 1; i++)
    {
        int hid = i * w;
        int j = 0;
        for (j = 1; j < w - 1; j++)
        {
            int wid = j;

            if(whitePlaneP[hid + wid] == T1)
                break;
        }
        if(j >= w - 2)
            continue;

        j++;
        int tj = j;
        while(whitePlaneP[hid + tj] != T1 && tj < w)
        {
            tj++;
        }
        if(tj < w -1)
            while(whitePlaneP[hid + j] != T1 && j < w)
            {
                if(whitePlaneP[hid + j] < T2 && whitePlaneP[hid + j] >= 0)
                    whitePlaneP[hid + j] = V;
                j++;
            }

    }
}

//void Is_shadow(unsigned char * color, int w, int h)
//{
//    for(int i = 0; i < h; i++)
//    {
//        int hid = i * w;
//        for(int j = 0; j < w; j++)
//        {
//            int wid = j;
//            if(color[hid + wid] < 50)
//            {
//                unsigned char R = planePRGB[hid * 3 + wid * 3 + 0];
//                unsigned char G = planePRGB[hid * 3 + wid * 3 + 1];
//                unsigned char B = planePRGB[hid * 3 + wid * 3 + 2];
//
//                float minRGB = Min(R, Min(G, B));
//                float maxRGB = Max(R, Max(G, B));
//
//                if(maxRGB - minRGB < 30)
//                    whitePlaneP[hid + wid] = 255;
//
//            }
//        }
//    }
//}
/*///////////////////////////////////////////////////////////////////////////
 * 功能描述：将识别的颜色区域生成一个颜色对像
 * 参数描述：color 标记好颜色区域的缓存， 缓存的宽 高 及缓存的指定色
 * 作者：郭伟东
 * 创建日期：2016年4月13日 22:01:00
 * 完成日期：
 * 这是最难写的部分，比较伤脑筋
/*///////////////////////////////////////////////////////////////////////////
/*///////////////////////////////////////////////////////////////////////////
 * 修改目的：
 * 修改描述：
 * 修改人：
 * 修改日期：
 * 完成日期：
/*///////////////////////////////////////////////////////////////////////////

void GetBolckCenterH(unsigned char * color, int w, int h, unsigned char V, int id, int & setID)
{
    int wid = id;

    for(int j = 0; j < h; j++)
    {
        int hid = j * w;

        if(hScanLine[j] > 0)
        {
            int s = j, e = j;
            for(int ii = j; ii > 0; ii--)
            {
                if(color[ii * w + wid] == V)
                    colorIDs[ii * w + wid] = hScanLine[j];
                else
                {
                    s = ii;
                    break;
                }
            }
            for(int ii = j+1; ii < w; ii++)
            {
                if(color[ii * w + wid] == V)
                    colorIDs[ii * w + wid] = hScanLine[j];
                else
                {
                    e = ii;
                    j = ii;
                    break;
                }
            }
        }
    }

    if(id + 1 < w)
    {
        wid = id + 1;
        int s = h, e = 0;
        for(int j = 1; j < h - 1; j++)
        {
            int hid = j * w;
            int hid0 = (j - 1) * w;
            int hid1 = (j + 1) * w;
            if(color[hid + wid] == V && color[hid0 + wid ] < V)
            {
                s = j;
                //colorIDs[hid + wid] = 200;
            }

            if(color[hid + wid] == V && color[hid1 + wid] < V)
            {
                e = j;
                int nid = -1;
                for(int jj = s; jj < e; jj++)
                {
                    if(hScanLine[jj] != 0)
                    {
                        nid = hScanLine[jj];
                        hScanLine[jj] = 0;
                        break;
                    }
                    //colorIDs[jj * w + wid] = 255;
                }
                colorIDs[e * w + wid] = 200;
                if(nid == -1)
                    nid = ++setID;
                //创建新ID
                int c = (s + e) / 2;
                hScanLine[c] = nid;

                //colorIDs[c * w + wid] = 255;
            }

//            if(j > s)
//                colorIDs[hid + wid] = 255;
        }

    }

}
void GetBolckCenter(unsigned char * color, int w, int h, unsigned char V, int id, int & setID)
{
    //一扫描指定颜色V
    //
//    memset(colorIDs, 0, w * h);
//    memset(wScanLine, 0, w);
    //for (int i = 0; i < h; i++)
    {
        int hid = id * w;

//        for(int j = 0; j < w; j++)
//        {
//            if(color[hid + j] == V)
//                colorIDs[hid + j] = 255;
//        }

        for(int j = 0; j < w; j++)
        {
            int wid = j;
            if(wScanLine[j] > 0)
            {
                int s = j, e = j;
                for(int ii = j; ii > 0; ii--)
                {
                    if(color[hid + ii] == V)
                        colorIDs[hid + ii] = wScanLine[j];
                    else
                    {
                        s = ii;
                        break;
                    }
                }
                for(int ii = j+1; ii < w; ii++)
                {
                    if(color[hid + ii] == V)
                        colorIDs[hid + ii] = wScanLine[j];
                    else
                    {
                        e = ii;
                        j = ii;
                        break;
                    }
                }
            }
        }

        if(id + 1 < h)
        {
            hid = (id + 1) * w;
            int s = w, e = 0;
            for(int j = 1; j < w - 1; j++)
            {
                if(color[hid + j] == V && color[hid + j - 1] < V)
                    s = j;
                if(color[hid + j] == V && color[hid + j + 1] < V)
                {
                    e = j;
                    int nid = -1;
                    for(int jj = s; jj < e; jj++)
                    {
                        if(wScanLine[jj] != 0)
                        {
                            nid = wScanLine[jj];
                            wScanLine[jj] = 0;
                            break;
                        }
                        //colorIDs[hid + jj] = 255;
                    }
                    if(nid == -1)
                        nid = ++setID;
                    //创建新ID
                    int c = (s + e) / 2;
                    wScanLine[c] = nid;
                }
            }

        }

        //wScanLineID;
        return;


//        for (int j = 1; j < w - 1; j++)
//        {
//            int wid = j;
//
//            if(whitePlaneP[hid + wid] == V)
//            {
//                if(wScanLine[j] == 0)
//                    wScanLine[j] = 1;
//                colorIDs[hid + wid] = 255;
//            }
//            else
//                wScanLine[j] = 0;

       // }
       // memset(wScanLine, 0, w);
    }
}

int minX = 0;
int maxX = 0;
int minY = 0;
int maxY = 0;
void BottonSide(unsigned char * color, int w, int h, unsigned char T, unsigned char V)
{
    int cx = w/2;
    int cy = h/2;
    minX = w/2;
    maxX = w/2;
    minY = h/2;
    maxY = h/2;

    int trL = 0;
    for(int i = cy; i > 0; i--)
    {
        int hid = i * w;
        for(int j = w/2; j < w; j++)
        {
            int wid = j;
            if(whitePlaneP[hid + wid] == T)
            {
                if(j > maxX)
                    maxX = j;
                if(i < minY)
                    minY = i;
                break;
            }
        }
        for(int j = w/2; j > 0; j--)
        {
            int wid = j;
            if(whitePlaneP[hid + wid] == T)
            {
                if(j < minX)
                    minX = j;
                if(i < minY)
                    minY = i;
                break;
            }
        }
    }
    for(int i = cy; i < h; i++)
    {
        int hid = i * w;
        for(int j = w/2; j < w; j++)
        {
            int wid = j;
            if(whitePlaneP[hid + wid] == T)
            {
                if(j > maxX)
                    maxX = j;
                if(i > maxY)
                    maxY = i;
                break;
            }
        }
        for(int j = w/2; j > 0; j--)
        {
            int wid = j;
            if(whitePlaneP[hid + wid] == T)
            {
                if(j < minX)
                    minX = j;
                if(i > maxY)
                    maxY = i;

                break;
            }
        }
    }

    int hidMin = minY * w;
    int hidMax = maxY * w;
    for (int j = minX; j < maxX; j++)
    {
        int wid = j;
        whitePlaneP[hidMin + wid] = V;
        whitePlaneP[hidMax + wid] = V;
    }

    for (int i = minY; i < maxY; i++)
    {
        int hid = i * w;
        whitePlaneP[hid + minX] = V;
        whitePlaneP[hid + maxX] = V;
    }

//    int Y = minY + (maxY - minY) / 5 * 2;
//    int hid = Y * w;
//    for (int j = minX; j < maxX; j++)
//    {
//        int wid = j;
//        whitePlaneP[hid + wid] = 255;
//    }
//    int X = minX + (maxX - minX) / 15*7.5;
//    for(int i = minY; i < maxY; i++)
//    {
//        int hid = i * w;
//        whitePlaneP[hid + X] = 255;
//    }

//    int hid = cy * w;
//    for (int j = minX; j < maxX; j++)
//    {
//        int wid = j;
//        whitePlaneP[hid + wid] = 255;
//    }
//
//    for(int i = minY; i < maxY; i++)
//    {
//        int hid = i * w;
//        whitePlaneP[hid + cx] = 255;
//    }

}

void GetColorBlock(unsigned char * color, int w, int h, int x, int y, unsigned char T, int &cT, int &cL, int &cR, int &cB)
{

   // whitePlaneP[y * w + x] = 255;

    int wid = x;
    for(int i = y; i > 0; i--)
    {
        int hid = i * w;
        if(whitePlaneP[hid + wid] == T)
        {
            cT = i;
            break;
        }
    }
    for(int i = y; i < h; i++)
    {
        int hid = i * w;
        if(whitePlaneP[hid + wid] == T)
        {
            cB = i;
            break;
        }
    }

    int hid = (cT + cB) / 2 * w;
    for(int j = x; j > 0; j--)
    {
        int wid = j;
        if(whitePlaneP[hid + wid] == T)
        {
            cL = j;
            break;
        }
    }
    for(int j = x; j < w; j++)
    {
        int wid = j;
        if(whitePlaneP[hid + wid] == T)
        {
            cR = j;
            break;
        }
    }
}

int colors1 = 0;
int colors2 = 0;
int colors3 = 0;
int colors4 = 0;
int colors5 = 0;
int colors6 = 0;
int colors7 = 0;
int colors8 = 0;
int colors9 = 0;
int colors10 = 0;
int colors11 = 0;
bool GetColor9(unsigned char * color, int w, int h, int x, int y, unsigned char T, unsigned char V)
{
    int hid = y * w;
    int wid = x;
    if(whitePlaneP[hid + wid] >= T)
        return false;

    //whitePlaneP[hid + wid] = 255;

    int c5T = y;
    int c5L = x;
    int c5R = x;
    int c5B = y;
    GetColorBlock(color, w, h, x, y, T, c5T, c5L, c5R, c5B);
//    whitePlaneP[c5T * w + wid] = 255;
//    whitePlaneP[hid + c5L] = 255;
//    whitePlaneP[hid + c5R] = 255;
//    whitePlaneP[c5B * w + wid] = 255;
    whitePlaneP[(c5T + c5B) / 2 * w + (c5L + c5R) / 2] = 255;

    colors[4].CX = (c5L + c5R) / 2 * sbl;
    colors[4].CY = (c5T + c5B) / 2 * sbl;
    colors[4].top = c5T * sbl;
    colors[4].left = c5L * sbl;
    colors[4].right = c5R * sbl;
    colors[4].botton = c5B * sbl;


    int y2 = c5T - (c5B - c5T) *0.8;
    hid = y2 * w;
    int c2T = y;
    int c2L = x;
    int c2R = x;
    int c2B = y;
    GetColorBlock(color, w, h, x, y2, T, c2T, c2L, c2R, c2B);
//    whitePlaneP[c2T * w + wid] = 255;
//    whitePlaneP[hid + c2L] = 255;
//    whitePlaneP[hid + c2R] = 255;
//    whitePlaneP[c2B * w + wid] = 255;
    whitePlaneP[(c2T + c2B) / 2 * w + (c2L + c2R) / 2] = 255;

    colors[1].CX = (c2L + c2R) / 2 * sbl;
    colors[1].CY = (c2T + c2B) / 2 * sbl;
    colors[1].top = c2T * sbl;
    colors[1].left = c2L * sbl;
    colors[1].right = c2R * sbl;
    colors[1].botton = c2B * sbl;


    int y8 = c5B + (c5B - c5T) *0.8;
    hid = y8 * w;
    int c8T = y;
    int c8L = x;
    int c8R = x;
    int c8B = y;
    GetColorBlock(color, w, h, x, y8, T, c8T, c8L, c8R, c8B);
//    whitePlaneP[c8T * w + wid] = 255;
//    whitePlaneP[hid + c8L] = 255;
//    whitePlaneP[hid + c8R] = 255;
//    whitePlaneP[c8B * w + wid] = 255;
    whitePlaneP[(c8T + c8B) / 2 * w + (c8L + c8R) / 2] = 255;

    colors[7].CX = (c8L + c8R) / 2 * sbl;
    colors[7].CY = (c8T + c8B) / 2 * sbl;
    colors[7].top = c8T * sbl;
    colors[7].left = c8L * sbl;
    colors[7].right = c8R * sbl;
    colors[7].botton = c8B * sbl;

    int x4 = c5L - (c5R - c5L) * 0.8;
    hid = y * w;
    wid = x4;
    int c4T = y;
    int c4L = x4;
    int c4R = x4;
    int c4B = y;
    GetColorBlock(color, w, h, x4, y, T, c4T, c4L, c4R, c4B);
//    whitePlaneP[c4T * w + wid] = 255;
//    whitePlaneP[hid + c4L] = 255;
//    whitePlaneP[hid + c4R] = 255;
//    whitePlaneP[c4B * w + wid] = 255;
    whitePlaneP[(c4T + c4B) / 2 * w + (c4L + c4R) / 2] = 255;

    colors[3].CX = (c4L + c4R) / 2 * sbl;
    colors[3].CY = (c4T + c4B) / 2 * sbl;
    colors[3].top = c4T * sbl;
    colors[3].left = c4L * sbl;
    colors[3].right = c4R * sbl;
    colors[3].botton = c4B * sbl;

    int x6 = c5R + (c5R - c5L) * 0.8;
    hid = y * w;
    wid = x6;
    int c6T = y;
    int c6L = x6;
    int c6R = x6;
    int c6B = y;
    GetColorBlock(color, w, h, x6, y, T, c6T, c6L, c6R, c6B);
//    whitePlaneP[c6T * w + wid] = 255;
//    whitePlaneP[hid + c6L] = 255;
//    whitePlaneP[hid + c6R] = 255;
//    whitePlaneP[c6B * w + wid] = 255;
    whitePlaneP[(c6T + c6B) / 2 * w + (c6L + c6R) / 2] = 255;

    colors[5].CX = (c6L + c6R) / 2 * sbl;
    colors[5].CY = (c6T + c6B) / 2 * sbl;
    colors[5].top = c6T * sbl;
    colors[5].left = c6L * sbl;
    colors[5].right = c6R * sbl;
    colors[5].botton = c6B * sbl;

    int y1 = y2;
    int x1 = x4;
    hid = y1 * w;
    wid = x1;
    int c1T = y1;
    int c1L = x1;
    int c1R = x1;
    int c1B = y1;
    GetColorBlock(color, w, h, x1, y1, T, c1T, c1L, c1R, c1B);
//    whitePlaneP[c1T * w + wid] = 255;
//    whitePlaneP[hid + c1L] = 255;
//    whitePlaneP[hid + c1R] = 255;
//    whitePlaneP[c1B * w + wid] = 255;
    whitePlaneP[(c1T + c1B) / 2 * w + (c1L + c1R) / 2] = 255;

    colors[0].CX = (c1L + c1R) / 2 * sbl;
    colors[0].CY = (c1T + c1B) / 2 * sbl;
    colors[0].top = c1T * sbl;
    colors[0].left = c1L * sbl;
    colors[0].right = c1R * sbl;
    colors[0].botton = c1B * sbl;

    int y3 = y2;
    int x3 = x6;
    hid = y3 * w;
    wid = x3;
    int c3T = y3;
    int c3L = x3;
    int c3R = x3;
    int c3B = y3;
    GetColorBlock(color, w, h, x3, y3, T, c3T, c3L, c3R, c3B);
//    whitePlaneP[c3T * w + wid] = 255;
//    whitePlaneP[hid + c3L] = 255;
//    whitePlaneP[hid + c3R] = 255;
//    whitePlaneP[c3B * w + wid] = 255;
    whitePlaneP[(c3T + c3B) / 2 * w + (c3L + c3R) / 2] = 255;

    colors[2].CX = (c3L + c3R) / 2 * sbl;
    colors[2].CY = (c3T + c3B) / 2 * sbl;
    colors[2].top = c3T * sbl;
    colors[2].left = c3L * sbl;
    colors[2].right = c3R * sbl;
    colors[2].botton = c3B * sbl;

    int y7 = y8;
    int x7 = x4;
    hid = y7 * w;
    wid = x7;
    int c7T = y7;
    int c7L = x7;
    int c7R = x7;
    int c7B = y7;
    GetColorBlock(color, w, h, x7, y7, T, c7T, c7L, c7R, c7B);
//    whitePlaneP[c7T * w + wid] = 255;
//    whitePlaneP[hid + c7L] = 255;
//    whitePlaneP[hid + c7R] = 255;
//    whitePlaneP[c7B * w + wid] = 255;
    whitePlaneP[(c7T + c7B) / 2 * w + (c7L + c7R) / 2] = 255;

    colors[6].CX = (c7L + c7R) / 2 * sbl;
    colors[6].CY = (c7T + c7B) / 2 * sbl;
    colors[6].top = c7T * sbl;
    colors[6].left = c7L * sbl;
    colors[6].right = c7R * sbl;
    colors[6].botton = c7B * sbl;

    int y9 = y8;
    int x9 = x6;
    hid = y9 * w;
    wid = x9;
    int c9T = y9;
    int c9L = x9;
    int c9R = x9;
    int c9B = y9;
    GetColorBlock(color, w, h, x9, y9, T, c9T, c9L, c9R, c9B);
//    whitePlaneP[c9T * w + wid] = 255;
//    whitePlaneP[hid + c9L] = 255;
//    whitePlaneP[hid + c9R] = 255;
//    whitePlaneP[c9B * w + wid] = 255;
    whitePlaneP[(c9T + c9B) / 2 * w + (c9L + c9R) / 2] = 255;

    colors[8].CX = (c9L + c9R) / 2 * sbl;
    colors[8].CY = (c9T + c9B) / 2 * sbl;
    colors[8].top = c9T * sbl;
    colors[8].left = c9L * sbl;
    colors[8].right = c9R * sbl;
    colors[8].botton = c9B * sbl;


    colors5 = (c6L + c6R) / 2;
    colors1 = ((c1L + c1R) / 2 + (c5L + c5R)/ 2) / 2 - 1;
    colors1 = colors1 - (colors5 - colors1);

    colors9 = colors5 + (colors5 - colors1);
    return true;
}


void GetColorS(unsigned char * color, int w, int h, int x, int y, unsigned char T, unsigned char V)
{
    whitePlaneP[y * w + colors1] = 255;

    colors3 = (colors1 + colors5) / 2;
    whitePlaneP[y * w + colors3] = 255;

    colors2 = (colors1 + colors3) / 2;
    whitePlaneP[y * w + colors2] = 255;

    colors4 = (colors3 + colors5) / 2;
    whitePlaneP[y * w + colors4] = 255;

    whitePlaneP[y * w + x] = 255;

    colors7 = (colors5 + colors9) / 2;
    whitePlaneP[y * w + colors7] = 255;

    colors6 = (colors5 + colors7) / 2;
    whitePlaneP[y * w + colors6] = 255;

    colors8 = (colors7 + colors9) / 2;
    whitePlaneP[y * w + colors8] = 255;

    whitePlaneP[y * w + colors9] = 255;

    colors11 = colors9 + (colors9 - colors7);
    colors10 = (colors9 + colors11) / 2;

    whitePlaneP[y * w + colors10] = 255;
    whitePlaneP[y * w + colors11] = 255;

    colors[9].CX = colors1 * sbl;
    colors[9].CY = y * sbl;
    colors[9].top = colors[9].CY - 20;
    colors[9].left = colors[9].CX - 20;
    colors[9].right = colors[9].CX + 20;
    colors[9].botton = colors[9].CY + 20;

    colors[10].CX = colors2 * sbl;
    colors[10].CY = y * sbl;
    colors[10].top = colors[10].CY - 20;
    colors[10].left = colors[10].CX - 20;
    colors[10].right = colors[10].CX + 20;
    colors[10].botton = colors[10].CY + 20;

    colors[11].CX = colors3 * sbl;
    colors[11].CY = y * sbl;
    colors[11].top = colors[11].CY - 20;
    colors[11].left = colors[11].CX - 20;
    colors[11].right = colors[11].CX + 20;
    colors[11].botton = colors[11].CY + 20;

    colors[12].CX = colors4 * sbl;
    colors[12].CY = y * sbl;
    colors[12].top = colors[12].CY - 20;
    colors[12].left = colors[12].CX - 20;
    colors[12].right = colors[12].CX + 20;
    colors[12].botton = colors[12].CY + 20;

    colors[13].CX = colors5 * sbl;
    colors[13].CY = y * sbl;
    colors[13].top = colors[13].CY - 20;
    colors[13].left = colors[13].CX - 20;
    colors[13].right = colors[13].CX + 20;
    colors[13].botton = colors[13].CY + 20;

    colors[14].CX = colors6 * sbl;
    colors[14].CY = y * sbl;
    colors[14].top = colors[14].CY - 20;
    colors[14].left = colors[14].CX - 20;
    colors[14].right = colors[14].CX + 20;
    colors[14].botton = colors[14].CY + 20;

    colors[15].CX = colors7 * sbl;
    colors[15].CY = y * sbl;
    colors[15].top = colors[15].CY - 20;
    colors[15].left = colors[15].CX - 20;
    colors[15].right = colors[15].CX + 20;
    colors[15].botton = colors[15].CY + 20;

    colors[16].CX = colors8 * sbl;
    colors[16].CY = y * sbl;
    colors[16].top = colors[16].CY - 20;
    colors[16].left = colors[16].CX - 20;
    colors[16].right = colors[16].CX + 20;
    colors[16].botton = colors[16].CY + 20;

    colors[17].CX = colors9 * sbl;
    colors[17].CY = y * sbl;
    colors[17].top = colors[17].CY - 20;
    colors[17].left = colors[17].CX - 20;
    colors[17].right = colors[17].CX + 20;
    colors[17].botton = colors[17].CY + 20;

    colors[18].CX = colors10 * sbl;
    colors[18].CY = y * sbl;
    colors[18].top = colors[18].CY - 20;
    colors[18].left = colors[18].CX - 20;
    colors[18].right = colors[18].CX + 20;
    colors[18].botton = colors[18].CY + 20;

    colors[19].CX = colors11 * sbl;
    colors[19].CY = y * sbl;
    colors[19].top = colors[19].CY - 20;
    colors[19].left = colors[19].CX - 20;
    colors[19].right = colors[19].CX + 20;
    colors[19].botton = colors[19].CY + 20;

}
//Out = Average + (In – Average) * ( 1 + percent) In 原始亮度，Average 平均亮度，Out 调整后的亮度，而percent即调整范围[-1,1]。http://blog.csdn.net/pizi0475/article/details/6740428
char cTable[256]; //<nipper>
unsigned char GetLevel(int in, int level)
{
    double delta = 1 + level;
    const int threshold = 128;
    return threshold + (in - threshold)* delta;
}

//参数brightness：可以从-255到255，如果亮度为负，图像变暗。
//参数contrast：可以从-100到100，中间值为0。
bool Light (long brightness , long contrast )
{
       float c=(100 + contrast)/100.0f;
       brightness+=128;


       for ( int i=0; i<256; i++) {
             cTable[ i] =  Max(0, Min(255,( int)(( i-128)* c + brightness + 0.5f)));
      }

}
//参数saturation：可以从100到100，正值增加饱和度
//参数colorspace：可1（HSL）或2（YUV）。
bool Saturate (const long saturation , const long colorspace )
{

    for (int i =0;i <256;i ++) {
        cTable[ i] = Max(0,Min (255,(int )((i -128)*(100 + saturation)/100.0f + 128.5f)));
    }

}

void YuvBuffTORgbBuff(unsigned char * yuvColor, int width, int height, unsigned char * rgbColor, int bit)
{
    int offset = width * height;
    // int offset2 = offset + offset / 4;
    for(int i = 0; i < height; i++)
    {
        int hid1 = i * width;
        int hid2 = i / 2 * width;
        for(int j = 0; j < width; j++)
        {
            int wid1 = j;
            int wid2 = j;

            unsigned char *Y = &yuvColor[hid1 + wid1];
            unsigned char *U = 0;
            unsigned char *V = 0;

            int uvid = j % 2 > 0.0 ? 1 : 0;

            if (uvid == 0) {
                V = &yuvColor[offset + hid2 + wid2 + 0];
                U = &yuvColor[offset + hid2 + wid2 + 1];
            }
            else {
                V = &yuvColor[offset + hid2 + wid2 - 1];
                U = &yuvColor[offset + hid2 + wid2 + 0];
            }

            buff3[i * width * 3 + j * 3 + buff3ID] = *Y;

            unsigned char *B = &rgbColor[i * width * 3 + j * 3 + 0];
            unsigned char *G = &rgbColor[i * width * 3 + j * 3 + 1];
            unsigned char *R = &rgbColor[i * width * 3 + j * 3 + 2];

            YUV_RGB(*Y, *U, *V, *R, *G, *B);
        }
    }
}

void rgbuffTOYuvBuff(unsigned char * rgbColor, int width, int height, int bit, unsigned char * yuvColor)
{
    int offset = width * height;

    for(int i = 0; i < height; i++)
    {
        int hid1 = i * width;
        int hid2 = i / 2 * width;
        for (int j = 0; j < width; j++)
        {
            int wid1 = j;
            int wid2 = j;

            unsigned char *Y = &yuvColor[hid1 + wid1];
            unsigned char *U = 0;
            unsigned char *V = 0;

            int uvid = j % 2 > 0.0 ? 1 : 0;

            if (uvid == 0) {
                V = &yuvColor[offset + hid2 + wid2 + 0];
                U = &yuvColor[offset + hid2 + wid2 + 1];
            }
            else {
                V = &yuvColor[offset + hid2 + wid2 - 1];
                U = &yuvColor[offset + hid2 + wid2 + 0];
            }

            unsigned char B = rgbColor[i * width * 3 + j * 3 + 0];
            unsigned char G = rgbColor[i * width * 3 + j * 3 + 1];
            unsigned char R = rgbColor[i * width * 3 + j * 3 + 2];

            RGB_YUV(B, G, R, *Y, *U, *V);
        }
    }
}

void ColorObtain::ColorLoacation(unsigned char * color, int width, int height, int bit)
{
    YuvBuffTORgbBuff(color, width, height, rgbBuff, bit);

    int hid = (height / 2) * width * 3;
    int wid = width / 4 * 3;

    unsigned char B = rgbBuff[hid + wid + 0];
    unsigned char G = rgbBuff[hid + wid + 1];
    unsigned char R = rgbBuff[hid + wid + 2];

    for(int i = 0; i < 200; i++)
    {
        int hhid = i * width * 3;
        rgbBuff[hhid + wid + 0] = 255;
    }

    colors[0].R = B;
    colors[0].G = G;
    colors[0].B = R;

    RGB_HSV(colors[0].R, colors[0].G, colors[0].B, colors[0].H, colors[0].S, colors[0].V);
    RGB_YUV(colors[0].R, colors[0].G, colors[0].B, colors[0].Y, colors[0].UR, colors[0].VB);

 //   PushColorLocation(colors);

    rgbuffTOYuvBuff(rgbBuff, width, height, bit, color);
}
//http://www.cnblogs.com/Imageshop/archive/2013/04/07/3006334.html
void ColorObtain::ColorEdit(unsigned char * color, int width, int height, int bit)
{
    //LOGI("c++  %d, %d, %d %s  ", width, height, bit, color);
    Light (255 , 100);
    int offset = width * height;
   // int offset2 = offset + offset / 4;
    for(int i = 0; i < height; i++)
    {
        int hid1 = i * width;
        int hid2 = i / 2 * width;
        for(int j = 0; j < width; j++)
        {
            int wid1 = j;
            int wid2 = j;

            unsigned char * Y = &color[hid1 + wid1];
            unsigned char * U = 0;
            unsigned char * V = 0;

            int uvid = j % 2 > 0.0 ? 1 : 0;

            if(uvid == 0)
            {
                V = &color[offset + hid2 + wid2 + 0];
                U = &color[offset + hid2 + wid2 + 1];
            }
            else
            {
                V = &color[offset + hid2 + wid2 - 1];
                U = &color[offset + hid2 + wid2 + 0];
            }

            buff3[i * width * 3 + j * 3 + buff3ID] = *Y;

            unsigned char * B = &rgbBuff[i * width * 3 + j * 3 + 0];
            unsigned char * G = &rgbBuff[i * width * 3 + j * 3 + 1];
            unsigned char * R = &rgbBuff[i * width * 3 + j * 3 + 2];

//            unsigned char R = 255, G = 0, B = 0;

            //*Y = cTable[*Y];
//            unsigned char yy =GetLevel(*Y, 1);
            YUV_RGB( *Y, *U, *V, *R, *G, *B);

//            unsigned char max = Max(Max(*R, *G), *B);
//            unsigned char min = Min(Min(*R, *G), *B);
//            unsigned char cc = (min + max) / 2;



//            RGB_YUV(R, G, B, *Y, *U, *V);

//            if(*Y > 150)
//                whitePlaneP[hid1 + wid1] = *Y;
//            else
//                whitePlaneP[hid1 + wid1] = 0;
        }
    }

    int centerX = width / 2;
    unsigned char centerPoint = color[height / 2 * width + width / 2];


    int w = (width / sbl);
    int h = (height / sbl);
    //取灰度值
//    for(int i = 0; i < w-1; i++)
//    {
//        int hid = i * w;
//        int hhid = i * sbl * width;
//        for(int j = 0; j < h-1; j++)
//        {
//            int wid = j;
//            int wwid = j * sbl;
//            unsigned char C = (color[hhid + wwid] + buff5X3[hid + wid + 0] + buff5X3[hid + wid + 1] + buff5X3[hid + wid + 2]) / 4;
//            buff5X3[hid + wid + buff5X3id] = C;
//        }
//    }
    //取灰度值
    for(int i = 0; i < height - sbl; i+= sbl)
    {
        int hid1 = i * width;
        int hid2 = (i / sbl) * w;
        for (int j = 0; j < width - sbl; j+= sbl)
        {
            int wid1 = j;
            int wid2 = j / sbl;

            //buff5X3[hid2 + wid2 + buff5X3id] = color[hid1 + wid1];

            unsigned char C = (color[hid1 + wid1] + buff5X3[hid2 + wid2 + 0] + buff5X3[hid2 + wid2 + 1] + buff5X3[hid2 + wid2 + 2]) / 4;
            //unsigned char rgb = (rgbBuff[hid1 * 3 + wid1 * 3 + 0] + rgbBuff[hid1 * 3 + wid1 * 3 + 0]+ rgbBuff[hid1 * 3 + wid1 * 3 + 0]) / 3;
//            unsigned char C = (rgb + buff5X3[hid2 + wid2 + 0] + buff5X3[hid2 + wid2 + 1] + buff5X3[hid2 + wid2 + 2]) / 4;

            buff5X3[hid2 + wid2 + buff5X3id] = color[hid1 + wid1];

            planePRGB[hid2 * 3 + wid2 * 3 + 0] = rgbBuff[hid1 * 3 + wid1 * 3 + 0];
            planePRGB[hid2 * 3 + wid2 * 3 + 1] = rgbBuff[hid1 * 3 + wid1 * 3 + 1];
            planePRGB[hid2 * 3 + wid2 * 3 + 2] = rgbBuff[hid1 * 3 + wid1 * 3 + 2];

//            if(C - centerPoint > -10 && C - centerPoint < 10)
//            {
//                for (int s = i; s < i + sbl; s++)
//                {
//                    for (int e = j; e < j + sbl; e++)
//                    {
//                        rgbBuff[s * width * 3 + e * 3 + 0] = 255;
//                        rgbBuff[s * width * 3 + e * 3 + 1] = C;
//                        rgbBuff[s * width * 3 + e * 3 + 2] = C;
//                    }
//                }
//            }
//            else
//            {
//                for (int s = i; s < i + sbl; s++)
//                {
//                    int hhid = s * width * 3;
//                    for (int e = j; e < j + sbl; e++)
//                    {
//                        int wwid = e * 3;
//                        unsigned char * R = &rgbBuff[hhid + wwid + 0];
//                        unsigned char * G = &rgbBuff[hhid + wwid + 1];
//                        unsigned char * B = &rgbBuff[hhid + wwid + 2];
//                        * R = * R - 100 > 0 ? * R - 100 : 0;
//                        * G = * G - 100 > 0 ? * G - 100 : 0;
//                        * B = * B - 100 > 0 ? * B - 100 : 0;
//                    }
//                }
//            }

        }
    }





    //寻边
    unsigned char step = 20;
    int value = step;

    SetWhitePlaneP(buff5X3, w, h, value);

    int testValue = value;//50
    value += step;//100

    SetWhitePlanePAddRGB(buff5X3, w, h, testValue, value);

    testValue = value;//100
    value += step;//150
    int outLineValue = value;

    IS_Outline(buff5X3, w, h, testValue, value);//色块边界

    testValue = value;//150
    value += step;//200

    IS_SideW(buff5X3, w, h, testValue, value);//白板宽向边界
    IS_SideH(buff5X3, w, h, testValue, value);//白板高向边界
    //Is_shadow(buff5X3, w, h);

    testValue = value;//150
    value += step;//200
    BottonSide(buff5X3, w, h, testValue, value);

    int X9 = minX + (maxX - minX) / 15.0*7.1;
    int Y9 = minY + (maxY - minY) / 5.0 * 2.0;


//    int hid = Y * w;
//    for (int j = minX; j < maxX; j++)
//    {
//        int wid = j;
//        whitePlaneP[hid + wid] = 255;
//    }
//
//    for(int i = minY; i < maxY; i++)
//    {
//        int hid = i * w;
//        whitePlaneP[hid + X] = 255;
//    }

    testValue = value;//200
    value += step;//250

    bool isTreuColor9 = GetColor9(buff5X3, w, h, X9, Y9, outLineValue, value);

    int XS = colors5;//(maxX - minX) / 15.0*6;
    int YS = minY + (maxY - minY) / 5.0 * 4;

//    {
//        int hid = YS * w;
//        for (int j = minX; j < maxX; j++) {
//            int wid = j;
//            whitePlaneP[hid + wid] = 255;
//        }
//
//        for (int i = minY; i < maxY; i++) {
//            int hid = i * w;
//            whitePlaneP[hid + XS] = 255;
//        }
//    }
    GetColorS(buff5X3, w, h, XS, YS, outLineValue, value);
    testValue = value;//200
    value += step;//250

    if(isTreuColor9)
    for(int i = 0; i < 20; i++)
    {
        int ii = (colors[i].top + colors[i].botton) / 2;
        int jj = (colors[i].left + colors[i].right) / 2;
        int hid = ii * width * 3;
        int wid = jj * 3;

        colors[i].R = 0;
        colors[i].G = 0;
        colors[i].B = 0;
        int R = 0,G = 0,B = 0;
        for(int bi = ii - 2; bi < ii + 2; bi++)
        {
            int hhid = bi * width * 3;
            for(int bj = jj - 2; bj < jj + 2; bj++)
            {
                int wwid = bj * 3;
                R += rgbBuff[hhid + wwid + 0];
                G += rgbBuff[hhid + wwid + 1];
                B += rgbBuff[hhid + wwid + 2];
            }
        }
        colors[i].R = R / 25;
        colors[i].G = G / 25;
        colors[i].B = B / 25;

//        colors[i].R = rgbBuff[hid + wid + 0];
//        colors[i].G = rgbBuff[hid + wid + 1];
//        colors[i].B = rgbBuff[hid + wid + 2];

        RGB_HSV(colors[i].R, colors[i].G, colors[i].B, colors[i].H, colors[i].S, colors[i].V);
        RGB_YUV(colors[i].R, colors[i].G, colors[i].B, colors[i].Y, colors[i].UR, colors[i].VB);
    }


    //IS_Block(buff5X3, w, h, testValue, step * 2, value);

    static int id = 0;
//    GetBolckCenter(whitePlaneP, w, h, value, id);
//    id++;
//    if(id > h)
//    {
//        memset(colorIDs, 0, w * h);
//        id = 0;
//    }
    int setID = 10;
    memset(colorIDs, 0, w * h);

//    for(int i = 0; i < h; i++)
//        GetBolckCenter(whitePlaneP, w, h, value, i, setID);

//    for(int i = 0; i < w; i++)
//        GetBolckCenterH(whitePlaneP, w, h, value, i, setID);






//    int gzidH = 0;
//    for (int i = 0; i < height - sbl; i+= sbl)
//    {
//        int gzidW = 0;
//        int hid = i * width * 3;
//        for (int j = 0; j < width - sbl; j+= sbl)
//        {
//            int wid = j * 3;
//
//            unsigned char c = (buff3[hid + wid + 0] + buff3[hid + wid + 1] + buff3[hid + wid + 2]) / 3;
//
//            for (int s = i; s < i + sbl; s++)
//            {
//                for (int e = j; e < j + sbl; e++)
//                {
//                    rgbBuff[s * width * 3 + e * 3 + 0] = c;
//                    rgbBuff[s * width * 3 + e * 3 + 1] = c;
//                    rgbBuff[s * width * 3 + e * 3 + 2] = c;
//                }
//            }
//            gzidW++;
//        }
//        gzidH++;
//    }

    int gzidH = 0;
    for (int i = 0; i < height - sbl; i+= sbl)
    {
        int gzidW = 0;
        int hid = i * width * 3;
        int hid2 = (i / sbl) * w;
        for (int j = 0; j < width - sbl; j+= sbl)
        {
            int wid = j * 3;
            int wid2 = j / sbl;

            unsigned char c = whitePlaneP[hid2 + wid2];
            //unsigned char c = planePRGB[hid2 * 3 + wid2 * 3];;

            //if(c < 255)
            for (int s = i; s < i + sbl; s++)
            {
                for (int e = j; e < j + sbl; e++)
                {
                    rgbBuff[s * width * 3 + e * 3 + 0] = c;
                    rgbBuff[s * width * 3 + e * 3 + 1] = c;
                    rgbBuff[s * width * 3 + e * 3 + 2] = c;
                }
            }
            gzidW++;
        }
        gzidH++;
    }

    gzidH = 0;
    for (int i = 0; i < height - sbl; i+= sbl)
    {
        int gzidW = 0;
        int hid = i * width * 3;
        int hid2 = (i / sbl) * w;
        for (int j = 0; j < width - sbl; j+= sbl)
        {
            int wid = j * 3;
            int wid2 = j / sbl;

            unsigned char c = colorIDs[hid2 + wid2];

            if(c > 0)
            for (int s = i; s < i + sbl; s++)
            {
                for (int e = j; e < j + sbl; e++)
                {
                    rgbBuff[s * width * 3 + e * 3 + 0] = c;
                    rgbBuff[s * width * 3 + e * 3 + 1] = c;
                    rgbBuff[s * width * 3 + e * 3 + 2] = c;
                }
            }
            gzidW++;
        }
        gzidH++;
    }



    int conX = 0;

    int top = 0;
    int left = 0;
    int right = 0;
    int botton = 0;
//    for (int i = 2; i < h-1; i++)
//    {
//        for (int j = 2; j < w-1; j++)
//        {
//            unsigned char c1 = (buff5X3[i * w + j + 0] + buff5X3[i * w + j + 1] + buff5X3[i * w + j + 2]) / 3;
//
//            if (c1 > 200)
//            {
//                top = i;
//                conX = j;
//                left = j;
//                right = j;
//
//                int wid = (j * sbl) * 3;
//                for(int ii = 0; ii < height; ii++)
//                {
//                    int hid1 = ii * width * 3;
//                    rgbBuff[hid1 + wid + 0] = 255;
//                    rgbBuff[hid1 + wid + 1] = 0;
//                    rgbBuff[hid1 + wid + 2] = 0;
//                }
//                int hid = (i * sbl) * width * 3;
//                for(int jj = 0; jj < width; jj++)
//                {
//                    rgbBuff[hid + jj * 3 + 0] = 255;
//                    rgbBuff[hid + jj * 3 + 1] = 0;
//                    rgbBuff[hid + jj * 3 + 2] = 0;
//                }
//                break;
//            }
//        }
//        if (top > 0)
//            break;
//    }

//    for(int i = 0; i < height; i++)
//    {
//        int hid1 = i * width;
//        for (int j = 0; j < width; j++)
//        {
//            int wid1 = j;
//            rgbBuff[hid1*3 + wid1*3 + 0] = whitePlaneP[hid1 + wid1];
//            rgbBuff[hid1*3 + wid1*3 + 1] = whitePlaneP[hid1 + wid1];// == 0 ? 255 : 0;
//            rgbBuff[hid1*3 + wid1*3 + 2] = whitePlaneP[hid1 + wid1];
//        }
//    }


//    int wid = width / 6 * 3;
//    for(int ii = 0; ii < height; ii++)
//    {
//        int hid1 = ii * width * 3;
//        rgbBuff[hid1 + wid + 0] = 255;
//        rgbBuff[hid1 + wid + 1] = 0;
//        rgbBuff[hid1 + wid + 2] = 0;
//    }
//    int hid = height / 2 * width * 3;
//    for(int jj = 0; jj < width; jj++)
//    {
//        rgbBuff[hid + jj * 3 + 0] = 255;
//        rgbBuff[hid + jj * 3 + 1] = 0;
//        rgbBuff[hid + jj * 3 + 2] = 0;
//    }

//    for(int i = 0; i < 360; i++)
//    {
//        int x = width / 6 + 100 * sin(i * 3.14 / 180);
//        int y = height / 2 + 100 * cos(i * 3.14 / 180);
//
//        int hid = y * width * 3;
//        int wid = x * 3;
//
//        rgbBuff[hid + wid + 0] = 255;
//        rgbBuff[hid + wid + 1] = 0;
//        rgbBuff[hid + wid + 2] = 0;
//    }


    for(int i = 0; i < height; i++)
    {
        int hid1 = i * width;
        int hid2 = i / 2 * width;
        for (int j = 0; j < width; j++)
        {
            int wid1 = j;
            int wid2 = j;

            unsigned char *Y = &color[hid1 + wid1];
            unsigned char *U = 0;
            unsigned char *V = 0;

            int uvid = j % 2 > 0.0 ? 1 : 0;

            if (uvid == 0) {
                V = &color[offset + hid2 + wid2 + 0];
                U = &color[offset + hid2 + wid2 + 1];
            }
            else {
                V = &color[offset + hid2 + wid2 - 1];
                U = &color[offset + hid2 + wid2 + 0];
            }

            unsigned char B = rgbBuff[i * width * 3 + j * 3 + 0];
            unsigned char G = rgbBuff[i * width * 3 + j * 3 + 1];
            unsigned char R = rgbBuff[i * width * 3 + j * 3 + 2];

            RGB_YUV(B, G, R, *Y, *U, *V);
        }
    }


    buff5X3id++;
    if(buff5X3id >= 3)
        buff5X3id = 0;

    buff3ID++;
    if(buff3ID >= buffNumber)
        buff3ID = 0;


    float objectWidth = maxY - minY;//宽度5.1厘米
    float objectHeight = maxX - minX;//高度15厘米
    float bl = objectHeight / objectWidth; //比值

    bool isTrue = false;
    if (bl > 2.7 && bl < 3.3)
    {
        isTrue = true;
    }


   // PushColorToJava(width, height, colors, 20, isTrue, minX * sbl, minY * sbl, maxX * sbl, maxY * sbl);
}

void YUV_RGB(unsigned char Y, unsigned char CR, unsigned char CB, unsigned char &R, unsigned char &G, unsigned char&B)
{
    int r = Y + 1.402 * (CR-128);
    int g = Y - 0.34414 * (CB-128) - 0.71414 * (CR-128);
    int b = Y + 1.772 * (CB-128);

    R = r>255? 255 : r<0 ? 0 : r;
    G = g>255? 255 : g<0 ? 0 : g;
    B = b>255? 255 : b<0 ? 0 : b;
}

void RGB_YUV(unsigned char R, unsigned char G, unsigned char B, unsigned char &Y, unsigned char &UR, unsigned char &VB)
{
    Y = 0.299 * R + 0.587 * G + 0.114 * B;
    UR = - 0.1678 * R - 0.3313 * G + 0.5 * B + 128;
    VB = 0.5 * R - 0.4187 * G - 0.0813 * B + 128;
}

void HSV_RGB(float h, float s, float v, unsigned char & R, unsigned char & G, unsigned char & B)
{
    int k;
    float aa, bb, cc, f;

    if (s <= 0.0)
    {
        B = G = B = v * 255;
    }
    else
    {
        if (h == 1.0)
        {
            h = 0.0;
        }
        h *= 6.0;
        k = floor(h);
        f = h - k;

        aa = v * (1.0 - s);
        bb = v * (1.0 - s * f);
        cc = v * (1.0 - s * (1.0 -f));

        float r, g, b;

        switch (k)
        {
            case 0: r = v; g = cc; b = aa; break;
            case 1: r = bb; g = v; b = aa; break;
            case 2: r = aa; g = v; b = cc; break;
            case 3: r = aa; g = bb; b = v; break;
            case 4: r = cc; g = aa; b = v; break;
            case 5: r = v; g = aa; b = bb; break;
        }

        R = r * 255.0;
        G = g * 255.0;
        B = b * 255.0;
    }
}

void RGB_HSV(unsigned char R, unsigned char G, unsigned char B, float &H, float &S, float &V)
{
    float min, max, delta,tmp;
    tmp = R>G?G:R;
    min = tmp>B?B:tmp;
    tmp = R>G?R:G;
    max = tmp>B?tmp:B;
    V = max; // v
    delta = max - min;
    if( max != 0 )
        S = delta / max; // s
    else
    {
        // r = g = b = 0 // s = 0, v is undefined
        S = 0;
        H = 0;
        return;
    }
    if (delta == 0){
        H = 0;
        return;
    }
    else if(R == max){
        if (G >= B)
            H = (G - B) / delta; // between yellow & magenta
        else
            H = (G - B) / delta + 6.0f;
    }
    else if( G == max )
        H = 2.0f + ( B - R ) / delta; // between cyan & yellow
    else if (B == max)
        H = 4.0f + ( R - G ) / delta; // between magenta & cyan
    H *= 60.0; // degrees
//    float minRGB = Min(R, Min(G, B));
//    float maxRGB = Max(R, Max(G, B));
//    float deltaRGB = maxRGB - minRGB;
//
//    V = maxRGB;
//
//    if (maxRGB != 0)
//    {
//        S = deltaRGB / maxRGB;
//    }
//    else
//        S = 0.0;
//    if (S < 0.0)
//        H = -1.0;
//    else {
//        if (R == maxRGB)
//            H = (G - B) / deltaRGB;
//        else if (G == maxRGB)
//            H = 2.0 + (B - R) / deltaRGB;
//        else if (B == maxRGB)
//            H = 4.0 + (R - G) / deltaRGB;
//        H *= 60.0;
//
//        if (H < 0.0)
//            H += 360;
//    }
}

//public class YuvToRGB {
//private static int R = 0;
//private static int G = 1;
//private static int B = 2;
//    //I420是yuv420格式，是3个plane，排列方式为(Y)(U)(V)
//public static int[] I420ToRGB(byte[] src, int width, int height){
//        int numOfPixel = width * height;
//        int positionOfV = numOfPixel;
//        int positionOfU = numOfPixel/4 + numOfPixel;
//        int[] rgb = new int[numOfPixel*3];
//            for(int i=0; i<height; i++){
//                int startY = i*width;
//                int step = (i/2)*(width/2);
//                int startU = positionOfV + step;
//                int startV = positionOfU + step;
//                for(int j = 0; j < width; j++){
//                    int Y = startY + j;
//                    int U = startU + j/2;
//                    int V = startV + j/2;
//                    int index = Y*3;
//                    RGB tmp = yuvTorgb(src[Y], src[U], src[V]);
//                    rgb[index+R] = tmp.r;
//                    rgb[index+G] = tmp.g;
//                    rgb[index+B] = tmp.b;
//                }
//            }
//
//            return rgb;
//        }
//
//        private static class RGB{
//        public int r, g, b;
//        }
//
//        private static RGB yuvTorgb(byte Y, byte U, byte V){
//            RGB rgb = new RGB();
//            rgb.r = (int)((Y&0xff) + 1.4075 * ((V&0xff)-128));
//            rgb.g = (int)((Y&0xff) - 0.3455 * ((U&0xff)-128) - 0.7169*((V&0xff)-128));
//            rgb.b = (int)((Y&0xff) + 1.779 * ((U&0xff)-128));
//            rgb.r =(rgb.r<0? 0: rgb.r>255? 255 : rgb.r);
//            rgb.g =(rgb.g<0? 0: rgb.g>255? 255 : rgb.g);
//            rgb.b =(rgb.b<0? 0: rgb.b>255? 255 : rgb.b);
//            return rgb;
//        }
//
//        //YV16是yuv422格式，是三个plane，(Y)(U)(V)
//        public static int[] YV16ToRGB(byte[] src, int width, int height){
//            int numOfPixel = width * height;
//            int positionOfU = numOfPixel;
//            int positionOfV = numOfPixel/2 + numOfPixel;
//            int[] rgb = new int[numOfPixel*3];
//                for(int i=0; i<height; i++){
//                    int startY = i*width;
//                    int step = i*width/2;
//                    int startU = positionOfU + step;
//                    int startV = positionOfV + step;
//                    for(int j = 0; j < width; j++){
//                        int Y = startY + j;
//                        int U = startU + j/2;
//                        int V = startV + j/2;
//                        int index = Y*3;
//                        //rgb[index+R] = (int)((src[Y]&0xff) + 1.4075 * ((src[V]&0xff)-128));
//                        //rgb[index+G] = (int)((src[Y]&0xff) - 0.3455 * ((src[U]&0xff)-128) - 0.7169*((src[V]&0xff)-128));
//                        //rgb[index+B] = (int)((src[Y]&0xff) + 1.779 * ((src[U]&0xff)-128));
//                        RGB tmp = yuvTorgb(src[Y], src[U], src[V]);
//                        rgb[index+R] = tmp.r;
//                        rgb[index+G] = tmp.g;
//                        rgb[index+B] = tmp.b;
//                    }
//                }
//                return rgb;
//            }
//
//            //YV12是yuv420格式，是3个plane，排列方式为(Y)(V)(U)
//            public static int[] YV12ToRGB(byte[] src, int width, int height){
//                int numOfPixel = width * height;
//                int positionOfV = numOfPixel;
//                int positionOfU = numOfPixel/4 + numOfPixel;
//                int[] rgb = new int[numOfPixel*3];
//
//                    for(int i=0; i<height; i++){
//                        int startY = i*width;
//                        int step = (i/2)*(width/2);
//                        int startV = positionOfV + step;
//                        int startU = positionOfU + step;
//                        for(int j = 0; j < width; j++){
//                            int Y = startY + j;
//                            int V = startV + j/2;
//                            int U = startU + j/2;
//                            int index = Y*3;
//
//                            //rgb[index+R] = (int)((src[Y]&0xff) + 1.4075 * ((src[V]&0xff)-128));
//                            //rgb[index+G] = (int)((src[Y]&0xff) - 0.3455 * ((src[U]&0xff)-128) - 0.7169*((src[V]&0xff)-128));
//                            //rgb[index+B] = (int)((src[Y]&0xff) + 1.779 * ((src[U]&0xff)-128));
//                            RGB tmp = yuvTorgb(src[Y], src[U], src[V]);
//                            rgb[index+R] = tmp.r;
//                            rgb[index+G] = tmp.g;
//                            rgb[index+B] = tmp.b;
//                        }
//                    }
//                    return rgb;
//                }
//
//                //YUY2是YUV422格式，排列是(YUYV)，是1 plane
//                public static int[] YUY2ToRGB(byte[] src, int width, int height){
//                    int numOfPixel = width * height;
//                    int[] rgb = new int[numOfPixel*3];
//                        int lineWidth = 2*width;
//                        for(int i=0; i<height; i++){
//                            int startY = i*lineWidth;
//                            for(int j = 0; j < lineWidth; j+=4){
//                                int Y1 = j + startY;
//                                int Y2 = Y1+2;
//                                int U = Y1+1;
//                                int V = Y1+3;
//                                int index = (Y1>>1)*3;
//                                RGB tmp = yuvTorgb(src[Y1], src[U], src[V]);
//                                rgb[index+R] = tmp.r;
//                                rgb[index+G] = tmp.g;
//                                rgb[index+B] = tmp.b;
//                                index += 3;
//                                tmp = yuvTorgb(src[Y2], src[U], src[V]);
//                                rgb[index+R] = tmp.r;
//                                rgb[index+G] = tmp.g;
//                                rgb[index+B] = tmp.b;
//                            }
//                        }
//                        return rgb;
//                    }
//
//                    //UYVY是YUV422格式，排列是(UYVY)，是1 plane
//                    public static int[] UYVYToRGB(byte[] src, int width, int height){
//                        int numOfPixel = width * height;
//                        int[] rgb = new int[numOfPixel*3];
//                            int lineWidth = 2*width;
//                            for(int i=0; i<height; i++){
//                                int startU = i*lineWidth;
//                                for(int j = 0; j < lineWidth; j+=4){
//                                    int U = j + startU;
//                                    int Y1 = U+1;
//                                    int Y2 = U+3;
//                                    int V = U+2;
//                                    int index = (U>>1)*3;
//                                    RGB tmp = yuvTorgb(src[Y1], src[U], src[V]);
//                                    rgb[index+R] = tmp.r;
//                                    rgb[index+G] = tmp.g;
//                                    rgb[index+B] = tmp.b;
//                                    index += 3;
//                                    tmp = yuvTorgb(src[Y2], src[U], src[V]);
//                                    rgb[index+R] = tmp.r;
//                                    rgb[index+G] = tmp.g;
//                                    rgb[index+B] = tmp.b;
//                                }
//                            }
//                            return rgb;
//                        }
//
//                        //NV21是YUV420格式，排列是(Y), (VU)，是2 plane
//                        public static int[] NV21ToRGB(byte[] src, int width, int height){
//                            int numOfPixel = width * height;
//                            int positionOfV = numOfPixel;
//                            int[] rgb = new int[numOfPixel*3];
//
//                                for(int i=0; i<height; i++){
//                                    int startY = i*width;
//                                    int step = i/2*width;
//                                    int startV = positionOfV + step;
//                                    for(int j = 0; j < width; j++){
//                                        int Y = startY + j;
//                                        int V = startV + j/2;
//                                        int U = V + 1;
//                                        int index = Y*3;
//                                        RGB tmp = yuvTorgb(src[Y], src[U], src[V]);
//                                        rgb[index+R] = tmp.r;
//                                        rgb[index+G] = tmp.g;
//                                        rgb[index+B] = tmp.b;
//                                    }
//                                }
//                                return rgb;
//                            }
//
//                            //NV12是YUV420格式，排列是(Y), (UV)，是2 plane
//                            public static int[] NV12ToRGB(byte[] src, int width, int height){
//                                int numOfPixel = width * height;
//                                int positionOfU = numOfPixel;
//                                int[] rgb = new int[numOfPixel*3];
//
//                                    for(int i=0; i<height; i++){
//                                        int startY = i*width;
//                                        int step = i/2*width;
//                                        int startU = positionOfU + step;
//                                        for(int j = 0; j < width; j++){
//                                            int Y = startY + j;
//                                            int U = startU + j/2;
//                                            int V = U + 1;
//                                            int index = Y*3;
//                                            RGB tmp = yuvTorgb(src[Y], src[U], src[V]);
//                                            rgb[index+R] = tmp.r;
//                                            rgb[index+G] = tmp.g;
//                                            rgb[index+B] = tmp.b;
//                                        }
//                                    }
//                                    return rgb;
//                                }
//
//                                //NV16是YUV422格式，排列是(Y), (UV)，是2 plane
//                                public static int[] NV16ToRGB(byte[] src, int width, int height){
//                                    int numOfPixel = width * height;
//                                    int positionOfU = numOfPixel;
//                                    int[] rgb = new int[numOfPixel*3];
//
//                                        for(int i=0; i<height; i++){
//                                            int startY = i*width;
//                                            int step = i*width;
//                                            int startU = positionOfU + step;
//                                            for(int j = 0; j < width; j++){
//                                                int Y = startY + j;
//                                                int U = startU + j/2;
//                                                int V = U + 1;
//                                                int index = Y*3;
//                                                RGB tmp = yuvTorgb(src[Y], src[U], src[V]);
//                                                rgb[index+R] = tmp.r;
//                                                rgb[index+G] = tmp.g;
//                                                rgb[index+B] = tmp.b;
//                                            }
//                                        }
//                                        return rgb;
//                                    }
//
//                                    //NV61是YUV422格式，排列是(Y), (VU)，是2 plane
//                                    public static int[] NV61ToRGB(byte[] src, int width, int height){
//                                        int numOfPixel = width * height;
//                                        int positionOfV = numOfPixel;
//                                        int[] rgb = new int[numOfPixel*3];
//
//                                            for(int i=0; i<height; i++){
//                                                int startY = i*width;
//                                                int step = i*width;
//                                                int startV = positionOfV + step;
//                                                for(int j = 0; j < width; j++){
//                                                    int Y = startY + j;
//                                                    int V = startV + j/2;
//                                                    int U = V + 1;
//                                                    int index = Y*3;
//                                                    RGB tmp = yuvTorgb(src[Y], src[U], src[V]);
//                                                    rgb[index+R] = tmp.r;
//                                                    rgb[index+G] = tmp.g;
//                                                    rgb[index+B] = tmp.b;
//                                                }
//                                            }
//                                            return rgb;
//                                        }
//
//                                        //YVYU是YUV422格式，排列是(YVYU)，是1 plane
//                                        public static int[] YVYUToRGB(byte[] src, int width, int height){
//                                            int numOfPixel = width * height;
//                                            int[] rgb = new int[numOfPixel*3];
//                                                int lineWidth = 2*width;
//                                                for(int i=0; i<height; i++){
//                                                    int startY = i*lineWidth;
//                                                    for(int j = 0; j < lineWidth; j+=4){
//                                                        int Y1 = j + startY;
//                                                        int Y2 = Y1+2;
//                                                        int V = Y1+1;
//                                                        int U = Y1+3;
//                                                        int index = (Y1>>1)*3;
//                                                        RGB tmp = yuvTorgb(src[Y1], src[U], src[V]);
//                                                        rgb[index+R] = tmp.r;
//                                                        rgb[index+G] = tmp.g;
//                                                        rgb[index+B] = tmp.b;
//                                                        index += 3;
//                                                        tmp = yuvTorgb(src[Y2], src[U], src[V]);
//                                                        rgb[index+R] = tmp.r;
//                                                        rgb[index+G] = tmp.g;
//                                                        rgb[index+B] = tmp.b;
//                                                    }
//                                                }
//                                                return rgb;
//                                            }
//
//                                            //VYUY是YUV422格式，排列是(VYUY)，是1 plane
//                                            public static int[] VYUYToRGB(byte[] src, int width, int height){
//                                                int numOfPixel = width * height;
//                                                int[] rgb = new int[numOfPixel*3];
//                                                    int lineWidth = 2*width;
//                                                    for(int i=0; i<height; i++){
//                                                        int startV = i*lineWidth;
//                                                        for(int j = 0; j < lineWidth; j+=4){
//                                                            int V = j + startV;
//                                                            int Y1 = V+1;
//                                                            int Y2 = V+3;
//                                                            int U = V+2;
//                                                            int index = (U>>1)*3;
//                                                            RGB tmp = yuvTorgb(src[Y1], src[U], src[V]);
//                                                            rgb[index+R] = tmp.r;
//                                                            rgb[index+G] = tmp.g;
//                                                            rgb[index+B] = tmp.b;
//                                                            index += 3;
//                                                            tmp = yuvTorgb(src[Y2], src[U], src[V]);
//                                                            rgb[index+R] = tmp.r;
//                                                            rgb[index+G] = tmp.g;
//                                                            rgb[index+B] = tmp.b;
//                                                        }
//                                                    }
//                                                    return rgb;
//                                                }
//                                            }
